// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KinectHorizontalDepthProfile.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Perception
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using System.Runtime.CompilerServices;
    using System.Runtime.Serialization;
    using System.Threading.Tasks;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Tracing;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.Cameras;

    /// <summary>
    /// Builds a horizontal depth profile using the kinect camera. 
    /// The depth profile retains the closest depth reading in each column of the Kinect image, reducing the depth image to a slice similar to a laser scan.
    /// </summary>
    [DataContract]
    public class KinectHorizontalDepthProfile : IHorizontalDepthProfile
    {
        /// <summary>
        /// The tracing context for this class.
        /// </summary>
        private static TraceContext tracer = TraceContexts.FromString(TraceContexts.Nav, "KinectHDP");

        /// <summary>
        /// A list containing a pre-computed trigonometric function for every column in the depth profile.
        /// </summary>
        [DataMember]
        private double[] inverseCosines;

        /// <summary>
        /// The depth values (measured along the ray).  Depth values are ordered left to right, just like the Kinect image.
        /// </summary>
        [DataMember]
        private double[] distances;

        /// <summary>
        /// The cached set of occupied points. 
        /// </summary>
        [DataMember]
        private Point2D[] points;

        /// <summary>
        /// The camera intrinsic parameters
        /// </summary>
        [DataMember]
        private DepthCameraIntrinsicParameters intrinsicParameters;

        /// <summary>
        /// The true center of the field of view.
        /// </summary>
        [DataMember]
        private double trueCenter;

        /// <summary>
        /// Initializes a new instance of the KinectHorizontalDepthProfile class.
        /// </summary>
        /// <param name="depthImage">The depth image to initialize from</param>
        /// <param name="pose">Pose of the Kinect camera in 3D (6 DOF) </param>
        /// <param name="panningSpeed">The speed at which the camera is rotating in the horizontal plane</param>
        /// <param name="intrinsicParameters">The depth camera intrinsic parameters</param>
        public KinectHorizontalDepthProfile(ImageFrameDepth depthImage, Pose pose, double panningSpeed, DepthCameraIntrinsicParameters intrinsicParameters)
            : this(new Pose2D(pose), intrinsicParameters)
        {
            // one millimeter in meters
            const double Millimeter = 0.001;

            // make sure we have the right size image
            if (depthImage.Width != intrinsicParameters.HorizontalResolution)
            {
                throw new ArgumentException(string.Format("Invalid depth frame resolution, expected {0}.", intrinsicParameters.HorizontalResolution));
            }

            Vector3 angles = pose.GetZYXEulerAngles();
            if (angles.X != 0)
            {
                // It's not clear how an HDP can be useful with a rolling camera (6 DOF)
                TraceOut.Error(tracer, "Roll detected! {0}. Pose: {1}", angles.X, pose);
                throw new ArgumentException("Support for camera roll not implemented.");
            }

            if (angles.Y != 0)
            {
                // Since we support camera tilt, we might want to support this at some point 
                // However, this would involve a few trigonometric operations per pixel. 
                // There is no point in paying this penalty until we actually need the functionality.
                TraceOut.Error(tracer, "Tilt detected! {0}. Pose: {1}", angles.Y, pose);
                throw new ArgumentException("Support for camera tilt not implemented.");
            }

            // compute any rolling shutter effect on each row
            double angleDuringShutter = panningSpeed * intrinsicParameters.RollingShutterReadoutTime;
            double rowShift = Math.Tan(angleDuringShutter / 2) * 2 * intrinsicParameters.HorizontalFocalLengthInPixels / depthImage.Height;

            // create a filter class to smooth the interesting pixels
            BilateralFilter filter = new BilateralFilter(1.4, 10, true);

            // iterate over each column and generate the depth profile
            int halfHeight = depthImage.Height / 2;
            ////for (int column = intrinsicParameters.DeadPixelCount; column < intrinsicParameters.HorizontalResolution; column++)
            Parallel.For(
                intrinsicParameters.DeadPixelCount,
                intrinsicParameters.HorizontalResolution,
                column =>
                {
                    bool hasValidReadings = false;
                    double minOccupiedDistance = short.MaxValue;
                    int rowMinDistance = 0;
                    int actualColumnMinDistance = 0;

                    // find the min distance in this column. 
                    for (int row = 0; row < depthImage.Height; row++)
                    {
                        int actualColumn = (int)(column + rowShift * (row - halfHeight) + 0.5);
                        if (actualColumn < 0 || actualColumn >= intrinsicParameters.HorizontalResolution)
                        {
                            continue;
                        }

                        int depth = depthImage[actualColumn, row];
                        hasValidReadings = hasValidReadings || (depth == DepthImageConstants.TooFar);
                        if (depth > 0)
                        {
                            hasValidReadings = true;
                            double depthInMeters = depth * Millimeter;
                            if (depthInMeters <= minOccupiedDistance)
                            {
                                minOccupiedDistance = depthInMeters;
                                actualColumnMinDistance = actualColumn;
                                rowMinDistance = row;
                            }
                        }
                    }

                    // as long as not all pixels in the column are floor, ceiling or no-reading, store the obstacle distance and lateral displacement (in meters)
                    if (hasValidReadings)
                    {
                        // smooth the pixel first
                        double smoothedMinDepth = filter.Apply(depthImage, actualColumnMinDistance, rowMinDistance) * Millimeter;
                        double distance = smoothedMinDepth * this.inverseCosines[actualColumnMinDistance];
                        this.distances[column] = distance;
                        double distanceFromFocalPointToPixel = Math.Sqrt((column - this.trueCenter) * (column - this.trueCenter) + intrinsicParameters.HorizontalFocalLengthInPixels * intrinsicParameters.HorizontalFocalLengthInPixels);
                        double lateralDisplacement = distance * (column - this.trueCenter) / distanceFromFocalPointToPixel;
                        double trueDepth = distance * intrinsicParameters.HorizontalFocalLengthInPixels / distanceFromFocalPointToPixel;
                        this.points[column] = new Point2D(trueDepth, lateralDisplacement);
                    }
                });
        }

        /// <summary>
        /// Initializes a new instance of the KinectHorizontalDepthProfile class.
        /// </summary>
        /// <param name="pose">Pose of the sensor in the horizontal plane</param>
        /// <param name="intrinsicParameters">The depth camera intrinsic parameters</param>
        public KinectHorizontalDepthProfile(Pose2D pose, DepthCameraIntrinsicParameters intrinsicParameters)
        {
            this.intrinsicParameters = intrinsicParameters;
            this.trueCenter = intrinsicParameters.HorizontalCenter + intrinsicParameters.DeadPixelCount / 2;
            this.Pose = pose;
            this.points = new Point2D[this.intrinsicParameters.HorizontalResolution];
            this.distances = new double[this.intrinsicParameters.HorizontalResolution];
            this.Initialize();
        }

        /// <summary>
        /// Initializes a new instance of the KinectHorizontalDepthProfile class.
        /// </summary>
        /// <param name="other">The instance to initialize from</param>
        internal KinectHorizontalDepthProfile(KinectHorizontalDepthProfile other)
        {
            this.intrinsicParameters = other.intrinsicParameters;
            this.trueCenter = other.trueCenter;
            this.Pose = other.Pose;
            this.points = new Point2D[this.intrinsicParameters.HorizontalResolution];
            this.distances = new double[this.intrinsicParameters.HorizontalResolution];
            this.inverseCosines = other.inverseCosines;
        }

        /// <summary>
        /// Initializes a new instance of the KinectHorizontalDepthProfile class.
        /// In support of tests only.
        /// </summary>
        /// <param name="depthToImagePlane">A single-line depth array to use instead of a full depth image</param>
        /// <param name="intrinsicParams">The depth camera intrinsic parameters to use</param>
        internal KinectHorizontalDepthProfile(short[] depthToImagePlane, DepthCameraIntrinsicParameters intrinsicParams)
            : this(new ImageFrameDepth(depthToImagePlane.Length, 1, depthToImagePlane), new Pose(), 0, intrinsicParams)
        {
        }

        /// <summary>
        /// Gets the number of entries (bins) in the horizontal depth profile.
        /// </summary>
        public int Length
        {
            get
            {
                return this.distances.Length;
            }
        }

        /// <summary>
        /// Gets the pose of the center of the horizontal depth profile.  
        /// </summary>
        [DataMember]
        public Pose2D Pose
        {
            get;
            private set;
        }

        /// <summary>
        /// Gets the angular field of view of the depth profile
        /// </summary>
        public double FOV
        {
            get { return this.intrinsicParameters.HorizontalFieldOfViewInRadians; }
        }

        /// <summary>
        /// Gets the maximum distance in the scan line
        /// </summary>
        public double MaximumDistance
        {
            get { return this.distances.Max(); }
        }

        /// <summary>
        /// Returns the maximum width of the field of view given a depth cutoff distance.
        /// </summary>
        /// <param name="maxDepth">The maximum depth distance</param>
        /// <returns>The width of the field of view at the given depth</returns>
        public double GetFOVWidthForDepth(double maxDepth)
        {
            return maxDepth * this.intrinsicParameters.HorizontalResolution * this.intrinsicParameters.InverseHorizontalFocalLengthInPixels;
        }

        /// <summary>
        /// Returns the angle of the ray corresponding to the given index.
        /// </summary>
        /// <param name="index">The index of a depth value of hit points</param>
        /// <returns>The corresponding angle.</returns>
        public double GetAngleForIndex(int index)
        {
            return Math.Atan2(index + 0.5 - this.trueCenter, this.intrinsicParameters.HorizontalFocalLengthInPixels);
        }

        /// <summary>
        /// Returns the distance to the closest obstacle along the specified ray (distance is measured to the focal point, i.e. along the ray). Depth values are ordered left to right, just like the Kinect image.
        /// </summary>
        /// <param name="index">Index of the ray / pixel</param>
        /// <returns>The distance to the obstacle along the given ray / pixel</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public double GetHitDistance(int index)
        {
            return this.distances[index];
        }

        /// <summary>
        /// Returns the closest occupied point in the sensor coordinate frame for the given ray / pixel index
        /// </summary>
        /// <param name="index">Index of the ray / pixel</param>
        /// <returns>The coordinates of the closest occupied point in the sensor coordinate frame</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Point2D GetHitPoint(int index)
        {
            return this.points[index];
        }

        /// <summary>
        /// Adds an observed obstacle specified in local coordinates.
        /// Has no effect if the profile already contains a smaller depth value at that heading.
        /// </summary>
        /// <param name="observation">The observation to add, in local sensor coordinates (x represents depth, along the optical axis, y is positive to the left of the optical axis).</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void AddHitPoint(Point2D observation)
        {
            double distance;
            int index = this.Reproject(observation, out distance);
            if (index < 0 || index >= this.intrinsicParameters.HorizontalResolution)
            {
                return;
            }

            if (observation.X < this.points[index].X || this.points[index].X == 0)
            {
                this.distances[index] = distance;
                this.points[index] = observation;
            }
        }

        /// <summary>
        /// Given a point, it returns the column it would project into and the distance along the ray.
        /// </summary>
        ///  <param name="localPoint">The point to re-project, in local coordinates</param>
        /// <param name="distance">The distance along the ray to the specified point</param>
        /// <returns>The index of the column the point would project to</returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public int Reproject(Point2D localPoint, out double distance)
        {
            if (localPoint.X < 0)
            {
                distance = 0;
                return -1;
            }

            double column = (localPoint.Y / localPoint.X) * this.intrinsicParameters.HorizontalFocalLengthInPixels + this.trueCenter;
            int index = (int)(column + 0.5);
            if (index < 0 || index >= this.intrinsicParameters.HorizontalResolution)
            {
                distance = 0;
                return -1;
            }

            distance = localPoint.X * this.inverseCosines[index];
            return index;
        }

        /// <summary>
        /// Creates an empty horizontal depth profile with the same pose and structure as this one.
        /// </summary>
        /// <returns>An empty depth profile</returns>
        public IHorizontalDepthProfile Create()
        {
            return new KinectHorizontalDepthProfile(this);
        }

        /// <summary>
        /// Initializes elements of the KinectHorizontalDepthProfile class that could be shared between multiple instances in the future.
        /// </summary>
        private void Initialize()
        {
            if (this.inverseCosines == null)
            {
                this.inverseCosines = new double[this.intrinsicParameters.HorizontalResolution];
                for (int column = 0; column < this.intrinsicParameters.HorizontalResolution; column++)
                {
                    double tan = (column - this.trueCenter) * this.intrinsicParameters.InverseHorizontalFocalLengthInPixels;
                    this.inverseCosines[column] = Math.Sqrt(1 + tan * tan);
                }
            }
        }
    }
}
